﻿/*****************************************************************************
** Includes
*****************************************************************************/

#include "RobotCmdNode.hpp"
#include "DlClient.h"
#include "JsonUtil.h"

/*****************************************************************************
** Namespaces
*****************************************************************************/
using namespace std::chrono_literals;
using std::placeholders::_1;

/*****************************************************************************
  ** Implementation
  *****************************************************************************/

RobotCmdNode::RobotCmdNode(DlClient *dl)
    : Node("dlclient_node", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true))
    , m_dl(dl)
{
    // m_pubCmd = this->create_publisher<pccmd_msg::msg::PcCmd>("PcCmdMsg",1);

    m_pubDlMsg = this->create_publisher<dl_msg::msg::DlMsg>("DlMsg", 10);

    getParameters();
    m_dl->connect2Host(m_strIp, m_usPort);
}

RobotCmdNode::~RobotCmdNode()
{
    rclcpp::shutdown();
}

void RobotCmdNode::getParameters()
{
    RCLCPP_INFO(this->get_logger(), "getParameters...");
    get_parameter_or(std::string("ip"), m_strIp, std::string("no"));
    get_parameter_or(std::string("port"), m_usPort, unsigned short(0));

    RCLCPP_INFO(this->get_logger(), "dl_client ip: %s,  port: %hu", m_strIp.c_str(), m_usPort);
}

void RobotCmdNode::pubDlMsg(double x, double y, double z, double rx, double ry, double rz, unsigned short state)
{
    auto dl_msg = dl_msg::msg::DlMsg();
    dl_msg.x = x;
    dl_msg.y = y;
    dl_msg.z = z;
    dl_msg.rx = rx;
    dl_msg.ry = ry;
    dl_msg.rz = rz;
    dl_msg.state = state;

    m_pubDlMsg->publish(dl_msg);

    RCLCPP_INFO(this->get_logger(), "send dl_msg, x: %f, y: %f, z: %f, state: %hu", x, y, z, state);
}
